#include "Arduino.h"
#include "BMP085.h"
#include "Wire.h"
#include "WString.h"


BMP085::BMP085(void) {
    _pADDR = 0x77;    // I2C Bus Address of BMP085 Pressure Sensor
    _pCTRL = 0xF4;    // BMP085 Control Register Address
    _pTEMP = 0x2E;    // BMP085 Start Temp Measurement
    _pDATA = 0xAA;    // BMP085 First Calibration Data Register Address
    _pATOD = 0xF6;    // BMP085 A/D Register Address
    _pMODE = 0xB4;    // BMP085 Start Press Measurement (oversampling = 2)

    ALT_OVRS = 2;     // BMP085 oversampling_setting
    ALT_SHFT = 6;     // BMP Oversampling multiplier shift (=8 - ALT_OVRS)

    alt_0 = 0;
    agl_0 = 0;
}

void BMP085::begin(void) {
  _init();
  alt_0 = 0;
  agl_0 = 0;
}

void BMP085::begin(long site_alt) {
  _init();
  alt_0 = 0;
  agl_0 = 0;
  alt_0 = site_alt - altitude();
  agl_0 = site_alt;
}

void BMP085::_init(void) {

  Wire.beginTransmission(_pADDR); // transmit to Altimeter
  Wire.write(_pDATA);        // send The cal data register address
  Wire.endTransmission();    // stop transmitting
  
  Wire.requestFrom(_pADDR, 22);
  byte MSB = Wire.read();
  AC1 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  AC2 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  AC3 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  AC4 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  AC5 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  AC6 = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  xB1  = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  B2  = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  MB  = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  MC  = (int(MSB) << 8) + Wire.read();
  MSB = Wire.read();
  MD  = (int(MSB) << 8) + Wire.read();
  
  // Get the BMP085 Temperature & Pressure
  start_temp();
  delay(5);
  get_temp();
  start_pressure();
  delay(30);
  p = pressure();
}

// -----------------------------------------------------------------------------
// get_temp() gets the temperature data from the BMP085 and sets the temp globals

void BMP085::start_temp(void) {
  Wire.beginTransmission(_pADDR); // transmit to Altimeter
  Wire.write(_pCTRL);        // Send A/D control register address
  Wire.write(_pTEMP);        // send the start temp command
  Wire.endTransmission();    // stop transmitting
  
}

void BMP085::get_temp(void) {
  
  Wire.beginTransmission(_pADDR); // transmit to Altimeter
  Wire.write(_pATOD);        // send The A/D data register address
  Wire.endTransmission();    // stop transmitting
  //delay(1);

  // Read Back Temp Value
  Wire.requestFrom(_pADDR, 2);
  byte MSB = Wire.read();  // receive MSB
  long UT = (long(MSB) << 8) + Wire.read();
  
  long X1 = ((UT - AC6) * AC5) >> 15;
  long X2 = (long(MC) << 11) / (X1 + MD);
  B5 = X1 + X2;
  T_alt = (B5 + 8) >> 4;
  
  return;
}

// -----------------------------------------------------------------------------
// start_pressure() starts the BMP085 pressure measurement

void BMP085::start_pressure(void) {
  Wire.beginTransmission(_pADDR); // transmit to Altimeter
  Wire.write(_pCTRL);        // Send A/D control register address
  Wire.write(_pMODE);        // send the start temp register address
  Wire.endTransmission();    // stop transmitting
}

// -----------------------------------------------------------------------------
// pressure() reads the BMP085 pressure measurement and returns the calibrated
//            pressure, in Pascals

long BMP085::pressure(void) {
  long p_temp = 0;
  
  Wire.beginTransmission(_pADDR); // transmit to Altimeter
  Wire.write(_pATOD);        // send The A/D data register address
  Wire.endTransmission();    // stop transmitting
  //delay(1);
  
  Wire.requestFrom(_pADDR, 3);

  byte MSB = Wire.read();  // recieve MSB
  byte LSB = Wire.read();  // recieve LSB
  long UP = ((long(MSB) << 16) + (long(LSB) << 8) + Wire.read()) >> ALT_SHFT;
  
  long B6 = B5 - 4000;
  long X1 = (long(B2) * ((B6 * B6) >> 12)) >> 11;
  long X2 = (long(AC2) * B6) >> 11;
  long X3 = X1 + X2;
  long B3 = ((((long(AC1) << 2) + X3) << ALT_OVRS) + 2) >> 2;
  X1 = (long(AC3) * B6) >> 13;
  X2 = (long(xB1) * ((B6 * B6) >> 12)) >> 16;
  X3 = (X1 + X2 + 2) >> 2;
  unsigned long B4 = (long(AC4) * long(X3 + 32768));
  B4 = B4 >> 15;
  unsigned long B7 = (UP - B3) * (50000 >> ALT_OVRS);
  if (B7 < 0x80000000) {
    p_temp = (B7 << 1) / B4;
  }
  else {
    p_temp = (B7/B4) << 1;
  }
  X1 = (p_temp >> 8) * (p_temp >> 8);
  X1 = (X1 * 3038) >> 16;
  X2 = (-7357 * p_temp) >> 16;
  p = p_temp + ((X1 + X2 + 3791) >> 4);
  
  return p;
}

long BMP085::altitude(void) {

  long a_temp = 131072L - (p << 7) / 99;
  a_temp = ((((((7006*a_temp)>>17)+3660)*a_temp)>>20)+1575)*a_temp;
  
  a_temp = ((17316*(a_temp >> 11)) >> 11);

  if (a_temp > 490000) {
    long c_temp = (a_temp - 490000) >> 11;
    a_temp += (c_temp * c_temp);
  }

  return a_temp - 370 + alt_0;
}

long BMP085::agl(void) {

  long a_temp = altitude() - agl_0;
  return a_temp;
}

String BMP085::ACconst(void) {
  String aStr = String(AC1) + ',' + String(AC2) + ',' + String(AC3) + ',';
  aStr += String(AC4) + ',' + String(AC5) + ',' + String(AC6);
  return aStr;
}

String BMP085::BMconst(void) {
  String aStr = String(xB1) + ',' + String(B2) + ',' + String(MB) + ',';
  aStr += String(MC) + ',' + String(MD) + ',' + String(B5);
  return aStr;
}

